#!/usr/bin/env python3
"""
Display the RobotOne arm in RViz with (optional) joint_state_publisher_gui.
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
    Command,
    FindExecutable,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition
from ament_index_python.packages import get_package_share_directory
from pathlib import Path


def generate_launch_description():
    use_gui_arg = DeclareLaunchArgument(
        "use_gui",
        default_value="false",
        description="Enable joint_state_publisher_gui sliders",
    )
    use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time",
        default_value="false",
        description="Use simulated clock if true",
    )

    xacro_file = PathJoinSubstitution(
        [FindPackageShare("arm"), "urdf", "robotone_arm.urdf.xacro"]
    )
    robot_description_content = Command(
        [
            FindExecutable(name="xacro"),
            " ",
            xacro_file,
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    rsp_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[robot_description, {"use_sim_time": LaunchConfiguration("use_sim_time")}],
        output="screen",
    )

    jsp_node = Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui",
        condition=IfCondition(LaunchConfiguration("use_gui")),
        parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}],
    )

    rviz_config = Path(
        get_package_share_directory("arm"), "rviz", "arm_display.rviz"
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", str(rviz_config)],
        parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}],
    )

    return LaunchDescription(
        [
            use_gui_arg,
            use_sim_time_arg,
            rsp_node,
            jsp_node,
            rviz_node,
        ]
    )

